#include "compass.hpp"
#include "sw_i2c.h"
#include "flamingdeath.h"

void compass_init() {
  int res;
  i2c_init();

  /* set 20Hz mode */
  res = i2c_write_byte(1, 0, 0x3C);
  print_str("b1:"); print_int(res); print_str("\r\n");
  res = i2c_write_byte(0, 0, 0x00);
  print_str("b2:"); print_int(res); print_str("\r\n");
  res = i2c_write_byte(0, 1, 0x14);
  print_str("b3:"); print_int(res); print_str("\r\n");

  /* set continuous mode */
  res = i2c_write_byte(1, 0, 0x3C);
  print_str("b4:"); print_int(res); print_str("\r\n");
  res = i2c_write_byte(0, 0, 0x02);
  print_str("b5:"); print_int(res); print_str("\r\n");
  res = i2c_write_byte(0, 1, 0x00);
  print_str("b6:"); print_int(res); print_str("\r\n");


}

void compass_read(int16_t *values) {
  int16_t mx;
  int16_t my;
  int16_t mz;
  int16_t ms;

  uint8_t res;
  res = i2c_write_byte(1, 0, 0x3D);
  //print_str("b7:"); print_int(res); print_str("\r\n");
  /* read x msb */
  res = i2c_read_byte(0, 0);
  mx = res;
  mx = mx << 8;
  res = i2c_read_byte(0, 0);
  mx += res;
  res = i2c_read_byte(0, 0);
  my = res;
  my = my << 8;
  res = i2c_read_byte(0, 0);
  my += res;
  res = i2c_read_byte(0, 0);
  mz = res;
  mz = mz << 8;
  res = i2c_read_byte(0, 0);
  mz += res;
  res = i2c_read_byte(1, 1);
  ms = res;

	// values as x,y,z,status
	values[0] = mx;
	values[1] = my;
	values[2] = mz;
	values[3] = ms;
  //print_str("x:"); print_int((int)mx); print_str(" y:"); print_int((int)my); print_str(" z:"); print_int((int)mz); print_str(" s:"); print_int((int)ms); print_str("\r\n");
}


///////////////////////////////////////////////////////
#include "compass.hpp"
#include <math.h>


// Public Methods //////////////////////////////////////////////////////////////

// Returns the number of degrees from the -Y axis that it
// is pointing.
float compass::heading() {
	return heading((vector){0.f,-1.f,0.f});
}

// Returns the number of degrees from the from vector that it
// is pointing.
float compass::heading(vector from) {
	// shift and scale
    m.x = (m.x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0;
    m.y = (m.y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0;
    m.z = (m.z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0;
	
	vector temp_a = a;
    // normalize
    vector_normalize(&temp_a);
    //vector_normalize(&m);
	
    // compute E and N
    vector E;
    vector N;
    vector_cross(&m,&temp_a,&E);
    vector_normalize(&E);
    vector_cross(&temp_a,&E,&N);
	
    // compute heading
    float head = correctionInDegrees + atan2f(vector_dot(&E,&from), vector_dot(&N,&from)) * 180.f/M_PI;
    if(head < 0.f) head += 360.f;
	return head;
}

float wrap(float angle) {
	if (angle > M_PI) 
		angle -= (2*M_PI);
	if (angle < -M_PI)
		angle += (2*M_PI);
	if (angle < 0)
		angle += 2*M_PI;
	return angle;
}

float compass::heading_pitch_roll(float pitch, float roll, formatted_io &BT) {
	// Takes in raw magnetometer values, pitch and roll and turns it into a 
	// tilt-compensated heading value ranging from -pi to pi (everything in this 
	// function should be in radians).
	m.x = (m.x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0;
    m.y = (m.y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0;
    m.z = (m.z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0;
	float mag = sqrtf(m.x*m.x+m.y*m.y+m.z*m.z);
	//find_extremes(BT);
	//return 0.0f;
	BT.print(m.x);
	BT.print("\t");
	BT.print(m.y);
	BT.print("\t");
	BT.print(m.z);
	BT.print("\t");
	BT.print(mag);
	BT.print("\r\n");
#if 0
	float tmp;
	tmp = roll;
	roll = pitch;
	pitch = tmp;
    float variation = correctionInDegrees*(M_PI/180.f); 
	float Xh = m.x * cosf(roll) + m.y * sinf(pitch) * sinf(roll) + m.z * cosf(pitch) * sinf(roll);
	float Yh = m.y * cosf(pitch) - m.z * sinf(pitch);
	BT.print(Xh);
	BT.print("\t");
	BT.print(Yh);
    BT.print("\t");
	float heading = atan2f(-Yh, Xh);
	BT.print((float)(heading*180/M_PI));
		BT.print("\r\n");
	return wrap(heading);	
#endif
	return 0.0f;
}

void compass::find_extremes(formatted_io& BT) {
	
	int16_t max_x = -1;
	int16_t max_y = -1;
	int16_t max_z = -1;
	
	int16_t min_x = -1;
	int16_t min_y = -1;
	int16_t min_z = -1;

	int16_t mag[4];
	
	while (1) {
		compass_read(mag);
		
		 if (max_x < mag[0])
		 max_x = mag[0];
		 else if (min_x > mag[0])
		 min_x = mag[0];
		 
		 if (max_y < mag[1])
		 max_y = mag[1];
		 else if (min_y > mag[1])
		 min_y = mag[1];
		 
		 if (max_z < mag[2])
		 max_z = mag[2];
		 else if (min_z > mag[2])
		 min_z = mag[2];
		 
		 BT.print(max_x);
		 BT.print("\t");
		 BT.print(max_y);
		 BT.print("\t");
		 BT.print(max_z);
		 BT.print("\t\t");
		 BT.print(min_x);
		 BT.print("\t");
		 BT.print(min_y);
		 BT.print("\t");
		 BT.print(min_z);
		 BT.print("\r\n");
	}
	
}

void vector_cross(const vector *a,const vector *b, vector *out) {
	out->x = a->y*b->z - a->z*b->y;
	out->y = a->z*b->x - a->x*b->z;
	out->z = a->x*b->y - a->y*b->x;
}

float vector_dot(const vector *a,const vector *b) {
	return a->x*b->x+a->y*b->y+a->z*b->z;
}

void vector_normalize(vector *a) {
	float mag = sqrt(vector_dot(a,a));
	a->x /= mag;
	a->y /= mag;
	a->z /= mag;
}
